package com.hitqz.robot.driver.listener;

import cn.hutool.core.util.StrUtil;
import cn.hutool.extra.spring.SpringUtil;
import com.google.common.util.concurrent.RateLimiter;
import com.hitqz.robot.api.business.enums.VoiceEnum;
import com.hitqz.robot.api.business.event.KcStopTriggerEvent;
import com.hitqz.robot.api.business.event.PlayVoiceEvent;
import com.hitqz.robot.api.common.dto.tickets.CtrlTicketsEvent;
import com.hitqz.robot.driver.communication.NetworkServiceThreadPool;
import com.hitqz.robot.driver.dto.KcLocationDto;
import com.hitqz.robot.driver.dto.KcNavStatus;
import com.hitqz.robot.driver.dto.KcRobotRunningStatus;
import com.hitqz.robot.driver.dto.KcStopStatusDto;
import com.hitqz.robot.driver.util.*;
import jakarta.annotation.PostConstruct;
import lombok.AllArgsConstructor;
import lombok.extern.slf4j.Slf4j;
import org.apache.commons.lang3.StringUtils;
import org.springframework.beans.factory.annotation.Value;
import org.springframework.context.event.EventListener;
import org.springframework.stereotype.Component;

import java.text.DecimalFormat;
import java.util.*;
import java.util.concurrent.atomic.AtomicInteger;
import java.util.stream.Collectors;
import java.util.stream.Stream;

@Component
@Slf4j
@AllArgsConstructor
public class KcSelfListener {

    private final static RateLimiter publishLowPowerLimiter = RateLimiter.create(1);

    private final static RateLimiter publishLidarStopLimiter = RateLimiter.create(1);

    private static float lastConfidence = 0.0f;
    private static int lastNavStatus;
    private static double lastPower = 0.0d;
    private static final AtomicInteger retryCount = new AtomicInteger(0);
    private static final AtomicInteger blockCount = new AtomicInteger(0);

    private final DecimalFormat df = new DecimalFormat("0.##");


    //急停状态处理
    @EventListener
    public void onStop(KcStopStatusDto event) {
        NetworkServiceThreadPool.dealEvent(() -> {
            KcLocationDto kcLocationDto=(KcLocationDto)KcCacheUtil.get(KcCacheUtil.KEY_LOCATION_STATUS);
            KcRobotRunningStatus runningStatus =(KcRobotRunningStatus) KcCacheUtil.get(KcCacheUtil.KEY_RUNNING_STATUS);
            if (kcLocationDto==null){
                return;
            }
            if (event.getHardStopStatus()==1){
                KcToolkit.targetPosCode = "";
                KcToolkit.publishWarningEvent("急停状态告警","硬急停触发,停止运动","","无",0);
                CtrlTicketsEvent ctrlTicketsEvent = new CtrlTicketsEvent();
                ctrlTicketsEvent.setType(CtrlTicketsEvent.TYPE_PAUSE);
                ctrlTicketsEvent.setMessage("触发硬急停按钮,任务暂停");
                ctrlTicketsEvent.setTicketsId(null);
                SpringUtil.getApplicationContext().publishEvent(ctrlTicketsEvent);
            }
            if (event.getFrontStopBarStatus()==1){
                if (runningStatus!=null && runningStatus.getTaskStatus()==2) {
                    log.info("[kc]前防撞条触发,停止队列");
                    KcToolkit.navToPosControl(1, "0");
                    KcToolkit.targetPosCode = "";
                }
                //碰到充电桩的情况.  充电房的定位精度较差 < 0.2就当到了  isReach是按<0.1判断的
                if (Objects.equals(kcLocationDto.getCurrentCode(),KcToolkit.chargePosCode) && Float.parseFloat(kcLocationDto.getDistance())<0.4f ){
                    //因为这边停止队列了，队列状态变退出，所以Handler里面队列完成执行充电的会触发不了，这边要下发充电
                    if ( runningStatus!=null && runningStatus.getIsCharging()==0 && runningStatus.getBatteryPower()< 0.85d) {
                        KcToolkit.targetPosCode = "";
                        KcToolkit.writeVar("cmd_a_charge", DataTypeEnum.INT, String.valueOf(1));
                        log.info("[kc]到达充电点，开始充电");
                    }
                }else if (runningStatus!=null && runningStatus.getTaskStatus()==2){
                    KcToolkit.publishWarningEvent("急停状态告警","前防撞条触发,停止运动","","无",0);
                    CtrlTicketsEvent ctrlTicketsEvent = new CtrlTicketsEvent();
                    ctrlTicketsEvent.setType(CtrlTicketsEvent.TYPE_PAUSE);
                    ctrlTicketsEvent.setMessage("触发前防撞条,任务暂停");
                    ctrlTicketsEvent.setTicketsId(null);
                    SpringUtil.getApplicationContext().publishEvent(ctrlTicketsEvent);
                }
            }
            if (event.getBackStopBarStatus()==1){
                if (runningStatus!=null && runningStatus.getTaskStatus()==2) {
                    log.info("[kc]后防撞条触发,停止队列");
                    KcToolkit.navToPosControl(1, "0");
                    KcToolkit.targetPosCode = "";
                    KcToolkit.publishWarningEvent("急停状态告警","后防撞条触发,停止运动","","无",0);
                    CtrlTicketsEvent ctrlTicketsEvent = new CtrlTicketsEvent();
                    ctrlTicketsEvent.setType(CtrlTicketsEvent.TYPE_PAUSE);
                    ctrlTicketsEvent.setMessage("触发后防撞条,任务暂停");
                    ctrlTicketsEvent.setTicketsId(null);
                    SpringUtil.getApplicationContext().publishEvent(ctrlTicketsEvent);
                }
            }
        });
    }

    //每6秒对机器人状态做业务逻辑处理
    @EventListener
    public void onListenStatus(KcRobotRunningStatus event){
        NetworkServiceThreadPool.dealEvent(()->{
            KcLocationDto kcLocationDto= (KcLocationDto)KcCacheUtil.get(KcCacheUtil.KEY_LOCATION_STATUS);
            if (kcLocationDto==null){
                kcLocationDto = new KcLocationDto();
                kcLocationDto.setDistance("999.9");
                kcLocationDto.setCurrentCode("0");
                kcLocationDto.setIsReach(false);
            }
            //播放行驶中的语音
            if (Math.abs(event.getForwardSpeed())>0.05d){
                PlayVoiceEvent playVoiceEvent = new PlayVoiceEvent();
                playVoiceEvent.setVoiceEnum(VoiceEnum.RUNNING);
                SpringUtil.getApplicationContext().publishEvent(playVoiceEvent);
            }
            //避障触发语音
            int taskStatus = event.getTaskStatus();
            //置信度   排除导航软件写入数据时变0的情况
            if (event.getConfidence()< 0.70d && lastConfidence > 0.70d &&  event.getConfidence() > 0.10d){
                //KcToolkit.publishWarningEvent("控制器告警","定位置信度"+df.format(event.getConfidence())+"过低","","请确定定位情况",2);
            }
            if (lastConfidence<0.1d && event.getConfidence()>0.8d && StrUtil.isNotBlank(KcToolkit.targetPosCode)){
                // 科聪导航的bug:它走着走着可能导航重启了，现象是停原地，关于导航的数据全变默认值，大概维持一分钟
                // 这边通过置信度突变去判断，给他重新下发点位，确保后台的流程可以继续
                KcToolkit.navToPosControl(0,KcToolkit.targetPosCode);
                log.info("[kc]导航置信度突变，重新下发点位,{}",KcToolkit.targetPosCode);
            }
            //导航状态
            if (lastNavStatus!= taskStatus && (taskStatus==KcNavStatusEnum.NAV_FAIL.getStatus()||taskStatus==KcNavStatusEnum.NAV_EXIT.getStatus()) ) {
                if (Objects.equals(KcToolkit.targetPosCode, kcLocationDto.getCurrentCode()) && kcLocationDto.getIsReach()){
                    KcToolkit.targetPosCode = "";
                    log.info("无需重下导航指令");
                } else if (retryCount.get()<3) {
                    KcToolkit.navToPosControl(0, KcToolkit.targetPosCode);
                    log.info("[kc]导航失败，重新下发点位,{},retryTimes:{}", KcToolkit.targetPosCode, retryCount.getAndIncrement());
                }else {
                    log.info("[kc]导航失败多次不在下发点位{}", KcToolkit.targetPosCode);
                    KcToolkit.targetPosCode = "";
                }
            }
            if (taskStatus==KcNavStatusEnum.NAV_FINISH.getStatus() || taskStatus==KcNavStatusEnum.NAV_NO_TASK.getStatus()){
                retryCount.set(0);
            }
            if (taskStatus==KcNavStatusEnum.NAV_GOING.getStatus() && event.getIsBlocked()!=0 && Objects.equals(kcLocationDto.getIsReach(),false)){
                int andIncrement = blockCount.getAndIncrement();
                log.info("[kc]导航过程中，障碍物阻塞次数:{}", andIncrement);
                if (andIncrement > 9){
                    PositionUtil positionUtil = SpringUtil.getBean("positionUtil");
                    boolean isNearPoint = positionUtil.checkPosition(event.getPositionX(), event.getPositionY());
                    if(!isNearPoint){
                        KcToolkit.navToPosControl(0, KcToolkit.targetPosCode);
                        log.info("[kc]导航过程中，障碍物阻塞次数过多，重新下发点位,{},blockCount:{}", KcToolkit.targetPosCode, andIncrement);

                        PlayVoiceEvent playVoiceEvent = new PlayVoiceEvent();
                        playVoiceEvent.setVoiceEnum(VoiceEnum.LIDAR_STOP);
                        SpringUtil.getApplicationContext().publishEvent(playVoiceEvent);
                    }
                    blockCount.set(0);

                }
            }
            if (event.getIsBlocked()==0){
                blockCount.set(0);
            }
            //低电量
            boolean isPowerFastCharge = false;
            if (Math.abs(lastPower-event.getBatteryPower()) > 0.1){
                log.warn("电量突变，舍弃，不告警");
                isPowerFastCharge = true;
            }else if (lastPower> 0.2d && event.getBatteryPower() < 0.2d){
                KcToolkit.publishWarningEvent("低电量告警","电量低于20%","","请及时充电",1);
            }else if (lastPower> 0.1d && event.getBatteryPower() < 0.1d){
                KcToolkit.publishWarningEvent("低电量告警","电量低于10%","","请及时充电",1);
            }
            //充电
            if (lastNavStatus!= taskStatus
                    && (taskStatus==KcNavStatusEnum.NAV_FINISH.getStatus()||taskStatus==KcNavStatusEnum.NAV_EXIT.getStatus())
                    && Objects.equals(kcLocationDto.getCurrentCode(),KcToolkit.chargePosCode)
                    && event.getIsCharging()==0){
                KcToolkit.writeVar("cmd_a_charge", DataTypeEnum.INT,String.valueOf(1));
                log.info("[kc]到达充电点，开始充电");
            }
            if (event.getIsCharging()==1 && KcToolkit.isAutoCharge){
                KcToolkit.isAutoCharge = false;
                log.info("[kc] 充上电了，关闭自主充电标志位");
            }


            //未充电的情况下,每5分钟检查电量状态 且电量不能是突变的
            if (!isPowerFastCharge && event.getIsCharging()!=1 && publishLowPowerLimiter.tryAcquire(60*5)){
                //0.2d 为默认低电量
                double chargeThreshold = KcToolkit.robot.getChargeThreshold() * 0.01d;
                double voiceThreshold = Math.max(chargeThreshold,0.2d);
                if (event.getBatteryPower() < voiceThreshold){
                    PlayVoiceEvent playVoiceEvent = new PlayVoiceEvent();
                    playVoiceEvent.setVoiceEnum(VoiceEnum.LOW_POWER);
                    SpringUtil.getApplicationContext().publishEvent(playVoiceEvent);
                }
                //自主充电事件
                if (event.getBatteryPower() < chargeThreshold){
                    if (KcToolkit.isAutoCharge){
                        log.info("已经在自主返回充电点...");
                    }else {
                        // 暂停当前任务
                        CtrlTicketsEvent ctrlTicketsEvent = new CtrlTicketsEvent();
                        ctrlTicketsEvent.setType(CtrlTicketsEvent.TYPE_PAUSE);
                        ctrlTicketsEvent.setMessage("触发自主充电,任务暂停");
                        ctrlTicketsEvent.setTicketsId(null);
                        SpringUtil.getApplicationContext().publishEvent(ctrlTicketsEvent);
                        try {
                            //这边的事件是6秒一次，所以里面加个小延迟没关系，避免暂停任务和返回充电的时序问题
                            Thread.sleep(1000);
                        } catch (InterruptedException e) {
                            throw new RuntimeException(e);
                        }
                        //报警
                        KcToolkit.publishWarningEvent("自主充电告警", "电量低于充电阈值" + KcToolkit.robot.getChargeThreshold() + "%,自主关闭任务并返回充电位置充电", "", "", 1);
                        //前往点位
                        KcToolkit.isAutoCharge = true;
                        KcToolkit.targetPosCode = StrUtil.isNotBlank(KcToolkit.chargePosCode)? KcToolkit.chargePosCode :"1";
                        KcToolkit.navToPosControl(0, StrUtil.isNotBlank(KcToolkit.chargePosCode)? KcToolkit.chargePosCode :"1");
                        log.info("[kc] 触发自主充电");
                    }
                }
                //充满电会自动退出充电,小于85要重新下发充电
                if (Objects.equals(kcLocationDto.getCurrentCode(),KcToolkit.chargePosCode)
                        && Float.parseFloat(kcLocationDto.getDistance())<0.3d
                        && event.getBatteryPower() < 0.85d
                        && event.getTaskStatus() != 2){
                    KcToolkit.writeVar("cmd_a_charge", DataTypeEnum.INT,String.valueOf(1));
                    log.info("[kc]在充电点，且电量<85%开始充电");
                }
            }
            lastNavStatus = taskStatus;
            lastConfidence = event.getConfidence();
            lastPower = event.getBatteryPower();
        });
    }

    @EventListener
    public void onListenWayPoint(KcNavStatus event){
        NetworkServiceThreadPool.dealEvent(()->{
            List<String> unPassedIds = new ArrayList<>();
            for (short unPassedId : event.getUnPassedPathPointIds()) {
                if (unPassedId != 0) {
                    unPassedIds.add(String.valueOf(unPassedId));
                }
            }
            if (unPassedIds.isEmpty()){
                unPassedIds.add("0");
            }
            List<String> passedIds = new ArrayList<>();
            for (short passedId : event.getPassedPathPointIds()) {
                if (passedId != 0) {
                    passedIds.add(String.valueOf(passedId));
                }
            }
            if (passedIds.isEmpty()){
                passedIds.add("0");
            }
            log.info("[kc]path unPass:{},pass:{}", unPassedIds, passedIds);
            dealOpenOrCloseDoor(unPassedIds, passedIds);



        });
    }


    private void dealOpenOrCloseDoor(List<String> unPassedIds, List<String> passedIds){
        try {
            //充电房门禁内点
            String chargeHomeInsideDoorPosCode = KcToolkit.chargeHomeInsideDoorPosCode;
            //充电房门禁外点
            String chargeHomeOutsideDoorPosCode = KcToolkit.chargeHomeOutsideDoorPosCode;
            //当前位置
            KcLocationDto kcLocationDto= (KcLocationDto)KcCacheUtil.get(KcCacheUtil.KEY_LOCATION_STATUS);
            if (kcLocationDto==null){
                kcLocationDto = new KcLocationDto();
                kcLocationDto.setDistance("999.9");
                kcLocationDto.setCurrentCode("0");
                kcLocationDto.setIsReach(false);
            }
            boolean openDoor = false;
            boolean closeDoor = false;
            //回来开门:前往充电房门禁内点，且已到达充电房门禁外点
            if (unPassedIds.contains(chargeHomeInsideDoorPosCode)){
                String lastPassedId = passedIds.get(passedIds.size()-1);
                if (lastPassedId.equals(chargeHomeOutsideDoorPosCode)){
                    openDoor = true;
                    log.info("[kc]前往充电房门禁内点，且已到达充电房门禁外点,开门");
                }
            }
            //回来关门:前往充电房门禁点，且离门禁点最近   充电房内有两个点，最里面那个是充电点且是关门禁点
            if (passedIds.contains(chargeHomeOutsideDoorPosCode)){
                String nextPassedId = unPassedIds.get(0);
                if (nextPassedId.equals(chargeHomeInsideDoorPosCode)){
                    closeDoor = true;
                    log.info("[kc]前往充电房门禁内点,且已进入充电房,关门");
                }
            }

            //出去开门:要出充电房
            if (Objects.equals(kcLocationDto.getCurrentCode(),chargeHomeInsideDoorPosCode) && unPassedIds.contains(chargeHomeOutsideDoorPosCode)){
                openDoor = true;
                log.info("[kc]前往充电房门禁外点,且在充电房内,开门");
            }
            //出去关门
            if (passedIds.contains(chargeHomeOutsideDoorPosCode) && !unPassedIds.contains(chargeHomeInsideDoorPosCode)){
                String lastPassedId = passedIds.get(passedIds.size()-1);
                if (lastPassedId.equals(chargeHomeOutsideDoorPosCode)){
                    closeDoor = true;
                    log.info("[kc]前往充电房门禁外点，且已到达充电房门禁外点,关门");
                }
            }
            if (openDoor){
                boolean openDoorSuccess = false;
                for (int i = 0; i < 3; i++) {
                    openDoorSuccess = M518Toolkit.openDoor();
                    if (openDoorSuccess){
                        log.info("路径中开门成功");
                        break;
                    }
                }
                KcRobotRunningStatus runningStatus = (KcRobotRunningStatus) KcCacheUtil.get(KcCacheUtil.KEY_RUNNING_STATUS);
                if (!openDoorSuccess && runningStatus.getTaskStatus() == KcNavStatusEnum.NAV_GOING.getStatus()) {
                    //KcToolkit.navToPosControl(1, "0");
                    //KcToolkit.targetPosCode = "";
                    log.info("路径中开门失败");
                }
            }
            if (closeDoor){
                M518Toolkit.closeDoor();
            }
        }catch (Exception e){
            log.error("[kc]处理前往站点中的开关门失败");
        }
    }








}
